using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;

namespace LocalizationTest
{
  public static class Globals
  {
    static public int stateSpaceMinimumX = -500;
    static public int stateSpaceMaximumX = +500;
    static public int stateSpaceMinimumY = -500;
    static public int stateSpaceMaximumY = +500;
    static public double stateSpaceMinimumRotation = -Math.PI;
    static public double stateSpaceMaximumRotation = +Math.PI;
    static public int stateSpaceInitialX = -150;
    static public int stateSpaceInitialY = +100;
    static public double stateSpaceInitialRotation;
    static public int rasterWidth = 5;
    static public double angleRasterWidth = 3.0 * Math.PI / 180.0;
    static public int motionRasterWidth = 5;
    static public double bearingMeasurementSigma = 0.1;
    static public double distanceMeasurementSigma = 30.0;
    static public double depthMeasurementSigma = 5.0;

    static public SelfLocatorProperties selfLocatorProperties = new SelfLocatorProperties();


    public static void Init(Experiment setup)
    {
      if(setup.simulatedWorldState is WorldState1D)
      {
        stateSpaceMinimumX = -1000;
        stateSpaceMaximumX = +1000;
        stateSpaceInitialX = -200;
        rasterWidth = 10;
        motionRasterWidth = 10;
        bearingMeasurementSigma = 0.1;
      }
      else if(setup.simulatedWorldState is WorldState2D)
      {
        stateSpaceMinimumX = -500;
        stateSpaceMaximumX = +500;
        stateSpaceMinimumY = -500;
        stateSpaceMaximumY = +500;
        stateSpaceInitialX = -50;
        stateSpaceInitialY = +100;
        rasterWidth = 35;
        motionRasterWidth = 25;
        bearingMeasurementSigma = 0.01;
      }
      else if (setup.simulatedWorldState is WorldStatePose2D)
      {
        stateSpaceMinimumX = -200;
        stateSpaceMaximumX = +200;
        stateSpaceMinimumY = -200;
        stateSpaceMaximumY = +200;
        stateSpaceMinimumRotation = -Math.PI;
        stateSpaceMaximumRotation = +Math.PI;
        stateSpaceInitialX = -100;
        stateSpaceInitialY = +100;
        stateSpaceInitialRotation = Math.PI / 3.0;
        rasterWidth = 10;
        motionRasterWidth = 5;
        angleRasterWidth = 5 * Math.PI / 180.0;
        bearingMeasurementSigma = 0.02;
      }
        /*
      else if (setup.simulatedWorldState is WorldStatePose2D)
      {
        stateSpaceMinimumX = -500;
        stateSpaceMaximumX = +500;
        stateSpaceMinimumY = -500;
        stateSpaceMaximumY = +500;
        stateSpaceMinimumRotation = -Math.PI;
        stateSpaceMaximumRotation = +Math.PI;
        stateSpaceInitialX = -100;
        stateSpaceInitialY = +100;
        stateSpaceInitialRotation = Math.PI / 3.0;
        rasterWidth = 40;
        motionRasterWidth = 40;
        angleRasterWidth = 45 * Math.PI / 180.0;
        bearingMeasurementSigma = 0.02;
      }*/
    }
    public static int PositionToIndex(double position, double rasterWidth, double minimum)
    {
      return (int)Math.Round((position - minimum) / rasterWidth);
    }
    public static double Gaussian(double x, double sigma, double mue)
    {
      return Math.Pow((2 * Math.PI * sigma * sigma), -0.5) * Math.Exp(-0.5 * Math.Pow((x - mue), 2) / (sigma * sigma));
    }

    public static List<FunctionWorldState> CreateListOfWorldStates(WorldState sampleWorldState)
    {
      List<FunctionWorldState> function = new List<FunctionWorldState>();
      if (sampleWorldState is WorldState1D)
      {
        int min = Globals.stateSpaceMinimumX;
        int max = Globals.stateSpaceMaximumX;
        for (int x = min; x <= max; x += rasterWidth)
        {
          function.Add(new FunctionWorldState(new WorldState1D(x), 10));
        }
      }
      if (sampleWorldState is WorldState2D)
      {
        Vector2 bottomLeftCorner = new Vector2(Globals.stateSpaceMinimumX, Globals.stateSpaceMinimumY);
        Vector2 topRightCorner = new Vector2(Globals.stateSpaceMaximumX, Globals.stateSpaceMaximumY);
        for (int x = (int)bottomLeftCorner.x; x <= (int)topRightCorner.x; x += rasterWidth)
        {
          for (int y = (int)bottomLeftCorner.y; y <= (int)topRightCorner.y; y += rasterWidth)
          {
            function.Add(new FunctionWorldState(new WorldState2D(x, y), 10));
          }
        }
      }
      if (sampleWorldState is WorldStatePose2D)
      {
        Vector2 bottomLeftCorner = new Vector2(Globals.stateSpaceMinimumX, Globals.stateSpaceMinimumY);
        Vector2 topRightCorner = new Vector2(Globals.stateSpaceMaximumX, Globals.stateSpaceMaximumY);
        for (int x = (int)bottomLeftCorner.x; x <= (int)topRightCorner.x; x += rasterWidth)
        {
          for (int y = (int)bottomLeftCorner.y; y <= (int)topRightCorner.y; y += rasterWidth)
          {
            for (double angle = Globals.stateSpaceMinimumRotation; angle <= Globals.stateSpaceMaximumRotation; angle += angleRasterWidth)
            {
              function.Add(new FunctionWorldState(new WorldStatePose2D(new Pose2D(x, y, angle)), 10));
            }
          }
        }
      }
      return function;
    }

    public static WorldState GetMinimum(WorldState sampleWorldState)
    {
      if (sampleWorldState is WorldState1D)
      {
        return new WorldState1D(Globals.stateSpaceMinimumX);
      }
      if (sampleWorldState is WorldState2D)
      {
        return new WorldState2D(Globals.stateSpaceMinimumX, Globals.stateSpaceMinimumY);
      }
      if (sampleWorldState is WorldStatePose2D)
      {
        return new WorldStatePose2D(Globals.stateSpaceMinimumX, Globals.stateSpaceMinimumY, Globals.stateSpaceMinimumRotation);
      }
      return null;
    }

    public static WorldState GetMaximum(WorldState sampleWorldState)
    {
      if (sampleWorldState is WorldState1D)
      {
        return new WorldState1D(Globals.stateSpaceMaximumX);
      }
      if (sampleWorldState is WorldState2D)
      {
        return new WorldState2D(Globals.stateSpaceMaximumX, Globals.stateSpaceMaximumY);
      }
      if (sampleWorldState is WorldStatePose2D)
      {
        return new WorldStatePose2D(Globals.stateSpaceMaximumX, Globals.stateSpaceMaximumY, Globals.stateSpaceMaximumRotation);
      }
      return null;
    }

    public static WorldState GetStep(WorldState sampleWorldState)
    {
      if (sampleWorldState is WorldState1D)
      {
        return new WorldState1D(Globals.rasterWidth);
      }
      if (sampleWorldState is WorldState2D)
      {
        return new WorldState2D(Globals.rasterWidth, Globals.rasterWidth);
      }
      if (sampleWorldState is WorldStatePose2D)
      {
        return new WorldStatePose2D(Globals.rasterWidth, Globals.rasterWidth, Globals.angleRasterWidth);
      }
      return null;
    }
  
  }
}
